#include "DynamicSimulator.h"
#include <stdio.h>

DynamicSimulator::DynamicSimulator(void)
{
	simplex = new ElementContainer();
}

DynamicSimulator::~DynamicSimulator(void)
{
}

void DynamicSimulator::LoadVolumetricMesh( char* name )
{
	loader.LoadTetMesh(simplex, name);

	printf("Vertex\n");

	for (int i=0;i<simplex->vertNum;i++)
	{
		Vertex* vert = simplex->vertices.at(i);

		printf("%f %f %f\n",vert->position[0],vert->position[1],vert->position[2]);
	}

	printf("Tet\n");

	for (int i=0;i<simplex->tetNum;i++)
	{
		Tet* t = simplex->tets.at(i);

		printf("%d %d %d %d\n",t->vertices[0]->index,t->vertices[1]->index,t->vertices[2]->index,t->vertices[3]->index);
	}
}

void DynamicSimulator::InitElement()
{
	InitMe();
	InitKe();
}

void DynamicSimulator::InitKe()
{
	float a[4], b[4], c[4], d[4];
	float Be[6][12];
	float De[6][6];
	float Inter[12][6];
	float sum;

	for (int i=0;i<6;i++)
	{
		for (int j=0;j<12;j++)
		{
			Be[i][j] = 0;
		}
	}

	for (int i=0;i<6;i++)
	{
		for (int j=0;j<6;j++)
		{
			De[i][j] = 0;
		}
	}

	// De

	float multiplier = E / ((1 + mu) * (1 - 2 * mu));

	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			if (i==j)
			{
				De[i][j] = (1 - mu) * multiplier;
			}else{
				De[i][j] = mu * multiplier;
			}
		}
	}

	De[3][3] = De[4][4] = De[5][5] = (1 - 2 * mu) * multiplier;


	for (int ti=0;ti<simplex->tetNum;ti++)
	{
		Tet* t = simplex->tets.at(ti);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		// Be
		CalDeterminant(a[0],b[0],c[0],d[0],vb,vc,vd);
		CalDeterminant(a[1],b[1],c[1],d[1],vc,vd,va);
		CalDeterminant(a[2],b[2],c[2],d[2],vd,va,vb);
		CalDeterminant(a[3],b[3],c[3],d[3],va,vb,vc);

		Be[0][0] = b[0];
		Be[0][3] = b[1];
		Be[0][6] = b[2];
		Be[0][9] = b[3];

		Be[1][1] = c[0];
		Be[1][4] = c[1];
		Be[1][7] = c[2];
		Be[1][10] = c[3];

		Be[2][2] = d[0];
		Be[2][5] = d[1];
		Be[2][8] = d[2];
		Be[2][11] = d[3];

		Be[3][0] = c[0];
		Be[3][3] = c[1];
		Be[3][6] = c[2];
		Be[3][9] = c[3];

		Be[3][1] = b[0];
		Be[3][4] = b[1];
		Be[3][7] = b[2];
		Be[3][10] = b[3];

		Be[4][2] = c[0];
		Be[4][5] = c[1];
		Be[4][8] = c[2];
		Be[4][11] = c[3];

		Be[4][1] = d[0];
		Be[4][4] = d[1];
		Be[4][7] = d[2];
		Be[4][10] = d[3];

		Be[5][0] = d[0];
		Be[5][3] = d[1];
		Be[5][6] = d[2];
		Be[5][9] = d[3];

		Be[5][2] = b[0];
		Be[5][5] = b[1];
		Be[5][8] = b[2];
		Be[5][11] = b[3];

		// Ke = Ve * Be~ * De * Be

		// Inter = Be~ * De

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<6;j++)
			{
				sum = 0;

				for (int k=0;k<6;k++)
				{
					sum += Be[k][i] * De[k][j];
				}

				Inter[i][j] = sum;
			}
		}

		// Ke = Ve * Inter * Be

		for (int i=0;i<12;i++)
		{
			for (int j=0;j<12;j++)
			{
				sum = 0;

				for (int k=0;k<6;k++)
				{
					sum += Inter[i][k] * Be[k][j];
				}

				t->Ke[i][j] = sum / (t->volume * 36);
			}
		}
		
	}
}

void DynamicSimulator::InitMe()
{
	float mass;

	for (int i=0;i<simplex->tetNum;i++)
	{
		Tet* t = simplex->tets.at(i);

		Vertex *va, *vb, *vc, *vd;
		va = t->vertices[0];
		vb = t->vertices[1];
		vc = t->vertices[2];
		vd = t->vertices[3];

		t->volume = abs((va - vd) * ((vb - vd) ^ (vc - vd)));
		t->volume /= 6.0;

		mass = DENSITY * 0.25 * t->volume;

		va->mass += mass;
		vb->mass += mass;
		vc->mass += mass;
		vd->mass += mass;
	}

}

void DynamicSimulator::CalDeterminant( float& a, float& b, float& c, float& d, Vertex* va, Vertex* vb, Vertex* vc )
{
	a = va->position[1] * vb->position[2] * vc->position[0] + va->position[2] * vb->position[0] * vc->position[1] + va->position[0] * vb->position[1] * vc->position[2] - (va->position[2] * vb->position[1] * vc->position[0] + va->position[0] * vb->position[2] * vc->position[1] + va->position[1] * vb->position[0] * vc->position[2]);

	b = va->position[1] * vb->position[2] + va->position[2] * vc->position[1] + vb->position[1] * vc->position[2] - (va->position[2] * vb->position[1] + va->position[1] * vc->position[2] + vb->position[2] * vc->position[1]);
	b *= -1;

	c = va->position[0] * vb->position[2] + va->position[2] * vc->position[0] + vb->position[0] * vc->position[2] - (va->position[2] * vb->position[0] + va->position[0] * vc->position[2] + vb->position[2] * vc->position[0]);

	d = va->position[1] * vb->position[0] + va->position[0] * vc->position[1] + vb->position[1] * vc->position[0] - (va->position[0] * vb->position[1] + va->position[1] * vc->position[0] + vb->position[0] * vc->position[1]);
}